#include "LOG.h"

void Log::addGPS(long lat, long lon, int confidence){
	
	workingPage.GPS_data_sample.lat = lat;
	workingPage.GPS_data_sample.lon = lon;
	workingPage.GPS_data_sample.DOP = confidence;
}


void Log::addIMU(int acclX, int acclY, int acclZ, int ptch, int rll, int yw, int hdng){
	workingPage.IMU_data_sample[IMU_record_idx].accelX = acclX;
	workingPage.IMU_data_sample[IMU_record_idx].accelY = acclY;
	workingPage.IMU_data_sample[IMU_record_idx].accelZ = acclZ;
	workingPage.IMU_data_sample[IMU_record_idx].pitch = ptch;
	workingPage.IMU_data_sample[IMU_record_idx].roll = rll;
	workingPage.IMU_data_sample[IMU_record_idx].yaw = yw;
	workingPage.IMU_data_sample[IMU_record_idx].heading = hdng;
	IMU_record_idx++;
	if(IMU_record_idx>=5){
		IMU_record_idx=0;
	}
}


void Log::printPage(int pgNum){

	//pPage = &workingPage;
	//wPage = &page2Print;

	//serialMode(serialMode_self);
	//Serial.println();
	//Serial.println(timeRxGPS[0]);

	//Serial.print(workingPage.GPS_data_sample.lat);
	//Serial.print(workingPage.GPS_data_sample.lon);
	//Serial.print(workingPage.GPS_data_sample.DOP);

	//for(int i=0; i<5; i++){
	//	Serial.print(workingPage.IMU_data_sample[i].accelX);
	//	Serial.print(" ");
	//	Serial.print(workingPage.IMU_data_sample[i].accelY);
	//	Serial.print(" ");	
	//	Serial.print(workingPage.IMU_data_sample[i].accelZ);
	//	Serial.print(" ");	
	//	Serial.print(workingPage.IMU_data_sample[i].pitch);
	//	Serial.print(" ");	
	//	Serial.print(workingPage.IMU_data_sample[i].roll);
	//	Serial.print(" ");	
	//	Serial.print(workingPage.IMU_data_sample[i].yaw);
	//	Serial.print(" ");	
	//	Serial.print(workingPage.IMU_data_sample[i].heading);
	//	Serial.print(" ");	
	//}
}
	
//#ifdef tst_PC
//	LogPage pageOut = SpiPROM.readLogPage(pgNum*def_LOG_PAGE_LEN);
//	Serial.println(pgNum);
//	Serial.println();
//	for(int i=0; i<def_LOG_RECS_PER_PAGE; i++){
//		Serial.println(pageOut.rec[i].head);
//			Serial.print(pageOut.rec[i].lat);
//			Serial.print(" / "); 
//			Serial.println(pageOut.rec[i].lon);
//			Serial.println(pageOut.rec[i].gpsTrust);
//			
//			Serial.print(pageOut.rec[i].accelX);
//			Serial.print(", ");
//			Serial.print(pageOut.rec[i].accelY);
//			Serial.print(", ");
//			Serial.println(pageOut.rec[i].accelZ);
//
//			Serial.print(pageOut.rec[i].pitch);
//			Serial.print(", ");
//			Serial.print(pageOut.rec[i].roll);
//			Serial.print(", ");
//			Serial.println(pageOut.rec[i].yaw);
//
//			Serial.println(pageOut.rec[i].heading);
//
//			Serial.println(pageOut.rec[i].temp);
//
//			Serial.println(pageOut.rec[i].timeStamp);
//			Serial.println(pageOut.rec[i].tail);
//			Serial.println("END");
//			Serial.println();
//		}
//			#endif
//
//}

LogPage Log::BuffToPage(LogBuff inBuff){
	LogPage retPage;
	//int bytePlace = 0;
			
	//for(int c=0; c<def_LOG_RECS_PER_PAGE; c++){
	//	bytePlace = c*def_LOG_REC_LEN;
	//	for(int b=0; b<def_LOG_REC_LEN; b++){
	//		switch(b){
	//		    case 0:                                         //uint8_t head (0)
	//					retPage.rec[c].head = inBuff.data[(b+bytePlace)];
 //                       break;
 //               case 1:                                         //long lat (1-4)
 //                       retPage.rec[c].lat = *((long*)(&inBuff.data[(b+bytePlace)]));
 //                       break;
 //               case 5:                                         //long lon (5-8)
 //                       retPage.rec[c].lon = *((long*)(&inBuff.data[(b+bytePlace)]));
 //                       break;
 //               case 9:                                         //int gpsTrust (9-10)
 //                       retPage.rec[c].gpsTrust = *((int*)(&inBuff.data[(b+bytePlace)]));
 //                       break;
 //               case 11:                                                //int accelX (11-12)
 //                       retPage.rec[c].accelX = *((int*)(&inBuff.data[(b+bytePlace)]));   
 //                       break;
 //               case 13:                                                //int accelY (13-14)
 //                       retPage.rec[c].accelY = *((int*)(&inBuff.data[(b+bytePlace)]));      
 //                       break;
 //               case 15:                                                //int accelZ (15-16)
 //                       retPage.rec[c].accelZ = *((int*)(&inBuff.data[(b+bytePlace)]));     
 //                       break;
 //               case 17:                                                //int pitch (17-18)
 //                       retPage.rec[c].pitch = *((int*)(&inBuff.data[(b+bytePlace)]));       
 //                       break;
 //               case 19:                                                //int roll (19-20)
 //                       retPage.rec[c].roll = *((int*)(&inBuff.data[(b+bytePlace)]));
 //                       break;
 //               case 21:                                                //int yaw (21-22)
 //                       retPage.rec[c].yaw = *((int*)(&inBuff.data[(b+bytePlace)])); 
 //                       break;
 //               case 23:                                                //int heading (23-24)
 //                       retPage.rec[c].heading = *((int*)(&inBuff.data[(b+bytePlace)]));  
 //                       break;
 //               case 25:                                                //int temp (25-26)
 //                       retPage.rec[c].temp = *((int*)(&inBuff.data[(b+bytePlace)]));
 //                       break;
 //               case 27:                                                //unsigned long timeStamp (27-30)
 //                       retPage.rec[c].timeStamp = *((unsigned long*)(&inBuff.data[(b+bytePlace)]));
 //                       break;
 //               case 31:                                                //uint8_t tail (31)
 //                       retPage.rec[c].tail = inBuff.data[(b+bytePlace)];
 //                       break;
 //               default:
 //                       break;
	//		}
	//	}
	//}
	return retPage;
}

LogBuff Log::PageToBuff(LogPage page){
	LogBuff retBuff;
	//uint8_t* ptrByte;
	//int bytePlace = 0;
	//for(int c=0; c<def_LOG_RECS_PER_PAGE; c++){
	//	bytePlace = c*def_LOG_REC_LEN;
	//	for(int b=0; b<def_LOG_REC_LEN; b++){
	//		switch(b){
	//			case 0:						//uint8_t head (0)
	//				ptrByte = (uint8_t*)&(page.rec[c].head);
	//				break;
	//			case 1:						//long lat (1-4)
	//				ptrByte = (uint8_t*)&(page.rec[c].lat);
	//				break;
	//			case 5:						//long lon (5-8)
	//				ptrByte = (uint8_t*)&(page.rec[c].lon);
	//				break;
	//			case 9:						//int gpsTrust (9-10)
	//				ptrByte = (uint8_t*)&(page.rec[c].gpsTrust);
	//				break;
	//			case 11:						//int accelX (11-12)
	//				ptrByte = (uint8_t*)&(page.rec[c].accelX);	
	//				break;
	//			case 13:						//int accelY (13-14)
	//				ptrByte = (uint8_t*)&(page.rec[c].accelY);	
	//				break;
	//			case 15:						//int accelZ (15-16)
	//				ptrByte = (uint8_t*)&(page.rec[c].accelZ);	
	//				break;
	//			case 17:						//int pitch (17-18)
	//				ptrByte = (uint8_t*)&(page.rec[c].pitch);	
	//				break;
	//			case 19:						//int roll (19-20)
	//				ptrByte = (uint8_t*)&(page.rec[c].roll);	
	//				break;
	//			case 21:						//int yaw (21-22)
	//				ptrByte = (uint8_t*)&(page.rec[c].yaw);	
	//				break;
	//			case 23:						//int heading (23-24)
	//				ptrByte = (uint8_t*)&(page.rec[c].heading);	
	//				break;
	//			case 25:						//int temp (25-26)
	//				ptrByte = (uint8_t*)&(page.rec[c].temp);	
	//				break;
	//			case 27:						//unsigned long timeStamp (27-30)
	//				ptrByte = (uint8_t*)&(page.rec[c].timeStamp);	
	//				break;
	//			case 31:						//uint8_t tail (31)
	//				ptrByte = (uint8_t*)&(page.rec[c].tail);
	//				break;
	//			default:
	//				break;
	//		}
	//		retBuff.data[(b+bytePlace)] = *ptrByte;
	//		ptrByte++;
		//}
	//}

	return retBuff;
}